/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SmartDashboard;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.Watchdog;

/**
 *
 * @author buttercream
 */
public class MaintenanceMode
{
    private static Joystick driverJoy;

    private static DriveMtrEncoder flEnc;
    private static DriveMtrEncoder frEnc;
    private static DriveMtrEncoder rlEnc;
    private static DriveMtrEncoder rrEnc;

    private static AngularEncoder flRrEnc;
    private static AngularEncoder frRlEnc;

    private static Victor flDrive;
    private static Victor frDrive;
    private static Victor rlDrive;
    private static Victor rrDrive;

    private static Jaguar flRrSteer;
    private static Jaguar frRlSteer;

    private static int FRONT_LEFT = 1;
    private static int FRONT_RIGHT = 2;
    private static int REAR_LEFT = 3;
    private static int REAR_RIGHT = 4;

    private static int FRONT_LEFT_BUTTON = 7;
    private static int FRONT_RIGHT_BUTTON = 8;
    private static int REAR_LEFT_BUTTON = 9;
    private static int REAR_RIGHT_BUTTON = 10;
    private static int EXIT_BUTTON = 2;

    public static void enterMaintenanceMode(
                                        Joystick stick,
                                        DriveMtrEncoder a_flEnc,
                                        DriveMtrEncoder a_frEnc,
                                        DriveMtrEncoder a_rlEnc,
                                        DriveMtrEncoder a_rrEnc,
                                        AngularEncoder a_flRrEnc,
                                        AngularEncoder a_frRlEnc,
                                        Victor a_flDrive,
                                        Victor a_frDrive,
                                        Victor a_rlDrive,
                                        Victor a_rrDrive,
                                        Jaguar a_flRrSteer,
                                        Jaguar a_frRlSteer)
    {

        flEnc = a_flEnc;
        frEnc = a_frEnc;
        rlEnc = a_rlEnc;
        rrEnc = a_rrEnc;

        flRrEnc = a_flRrEnc;
        frRlEnc = a_frRlEnc;

        flDrive = a_flDrive;
        frDrive = a_frDrive;
        rlDrive = a_rlDrive;
        rrDrive = a_rrDrive;

        flRrSteer = a_flRrSteer;
        frRlSteer = a_frRlSteer;
        driverJoy = stick;


        System.out.println("Maintenance mode entered.");
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kMain6, 1, "Maintenance mode entered.");
        DriverStationLCD.getInstance().updateLCD();

        Utils.pauseUntilReleased(driverJoy, Constants.MAINTENANCE_MODE_BUTTON_NUM);

        mainMenu();

        Utils.clearUserMessages();
        System.out.println("Maintenance mode exited.");
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kMain6, 1, "Maintenance mode exited.");
        DriverStationLCD.getInstance().updateLCD();
    }

    private static void mainMenu()
    {
        boolean quitMM = false;
        boolean refreshMM = true;

        while(!quitMM)
        {
            Watchdog.getInstance().feed();
            if(refreshMM)
            {
                System.out.println("Select one of the following with the joystick buttons.");
                System.out.println("7: Front-Left Wheel");
                System.out.println("8: Front-Right Wheel");
                System.out.println("9: Rear-Left Wheel");
                System.out.println("10: Rear-Right Wheel");
                System.out.println("2: Exit Maintenance Mode");

                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kMain6, 1, "Select one of the following with the joystick buttons.");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 1, "7: Front-Left Wheel");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser3, 1, "8: Front-Right Wheel");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser4, 1, "9: Rear-Left Wheel");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser5, 1, "10: Rear-Right Wheel");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser5, 1, "2: Exit Maintenance Mode");
                DriverStationLCD.getInstance().updateLCD();

                refreshMM = false;
            }

            if(driverJoy.getRawButton(FRONT_LEFT_BUTTON))
            {
                Utils.pauseUntilReleased(driverJoy, FRONT_LEFT_BUTTON);
                displaySubMenu(flDrive, flRrSteer, flRrEnc, "Front Left Wheel", false);

                refreshMM = true;
            }
            else if(driverJoy.getRawButton(FRONT_RIGHT_BUTTON))
            {
                Utils.pauseUntilReleased(driverJoy, FRONT_RIGHT_BUTTON);
                displaySubMenu(frDrive, frRlSteer, frRlEnc, "Front Right Wheel", true);

                refreshMM = true;
            }
            else if(driverJoy.getRawButton(REAR_LEFT_BUTTON))
            {
                Utils.pauseUntilReleased(driverJoy, REAR_LEFT_BUTTON);
                displaySubMenu(rlDrive, frRlSteer, frRlEnc, "Rear Left Wheel", false);
                refreshMM = true;
            }
            else if(driverJoy.getRawButton(REAR_RIGHT_BUTTON))
            {
                Utils.pauseUntilReleased(driverJoy, REAR_RIGHT_BUTTON);
                displaySubMenu(rrDrive, flRrSteer, flRrEnc, "Rear Right Wheel", true);
                refreshMM = true;
            }
            else if(driverJoy.getRawButton(1) || driverJoy.getRawButton( EXIT_BUTTON ))
            {
                Utils.pauseUntilReleased(driverJoy, 1);
                Utils.pauseUntilReleased(driverJoy,  EXIT_BUTTON);

                quitMM = true;
            }
        }

    }

    /**
     * As long as the trigger or 2 have not been pressed, set the speed of the
     * motor as the speed of the joystick.
     * @param mtr Motor to set the speed of
     * @param enc Motor's encoder
     * @return True if the trigger was pressed, false if the cancel button was
     * pressed
     */
    private static void jogSteerMotor(Jaguar mtr, AngularEncoder enc, boolean steering, boolean invertMtrSpeed, String name)
    {
        double Angle = 0.0;
        double lastAngle = 0.0;

        if(!steering)
        {
            while(Math.abs(driverJoy.getY()) > .1)
            {
                Watchdog.getInstance().feed();
                if(!invertMtrSpeed)
                {
                    mtr.set(-driverJoy.getY());
                }
                else
                {
                    mtr.set(driverJoy.getY());
                }
            }
        }
        else
        {
            while(Math.abs(driverJoy.getTwist()) > .1)
            {
                Watchdog.getInstance().feed();
        //        System.out.println("Twisting wheel: " + driverJoy.getTwist());
                mtr.set(-driverJoy.getTwist());

                Angle = enc.getAngle();

                printEncoderValues(enc);
                if( Angle != lastAngle )
                {
                    printEncoderValues(enc);
                    lastAngle = Angle;
                }
            }
        }


        mtr.set(0); //just to be safe
    }

    /**
     * Calibrate wheel position for one of the wheels
     * @param mtr Motor to calibrate
     * @param enc Encoder of the motor
     * @param wheelPos Integer from the RecalibrationValues class that represents
     * which wheel is being calibrated
     */
    private static void zeroEncoder(AngularEncoder enc)
    {
            enc.zeroAngle();
            System.out.println("Encoder has been zeroed.");
            DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser5, 1, "Encoder has been zeroed.");
            DriverStationLCD.getInstance().updateLCD();

    }

    /**
     * Print the raw value and current angle of the encoder.
     * @param enc Encoder
     */
    private static void printEncoderValues(AngularEncoder enc)
    {
        System.out.println("-------------------------------");

     //   System.out.println("Current angle: " + Utils.StringFormat61f(enc.getAngle()));

//        System.out.println("FL raw value : " + Utils.StringFormat6d(flEnc.getRawValue()));
//        System.out.println("FR raw value : " + Utils.StringFormat6d(frEnc.getRawValue()));
//        System.out.println("RL raw value : " + Utils.StringFormat6d(rlEnc.getRawValue()));
//        System.out.println("RR raw value : " + Utils.StringFormat6d(rrEnc.getRawValue()));
    }

    /**
     * Display the submenu just for the turn motors
     * @param mtr Motor to tune
     * @param enc Encoder of the motor
     * @param name Name of the motor
     * @param wheelPos Integer from the RecalibrationValues
     */
    private static void displaySubMenu(Victor driveMtr, Jaguar steerMtr, AngularEncoder enc, String name, boolean invertDriveMtr)
    {
        boolean exitSM = false;
        boolean refreshSM = true;
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kMain6, 1, name);

        while(!exitSM)
        {
            Watchdog.getInstance().feed();
            if(refreshSM)
            {
                System.out.println("\nSelect one of the following use the joystick buttons.");
                System.out.println("Y direction movement: Jog drive motor");
                System.out.println("Twist movement: Jog steering motor");
                System.out.println("4: Zero wheel");
                System.out.println("2: Return to Main Menu");

                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kMain6, 6, "Select one of the following use the joystick buttons.");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 6, "Y direction movement: Jog drive motor");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser3, 6, "Twist movement: Jog steering motor");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser4, 6, "4: Zero wheel");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser5, 6, "2: Return to Main Menu");
                DriverStationLCD.getInstance().updateLCD();
                refreshSM = false;
            }
            if(Math.abs(driverJoy.getY()) > .1)
            {
                //ToDO: Make a jogDriveMotor
             //   jogDriveMotor(driveMtr, null, false, invertDriveMtr, name);
                refreshSM = true;
            }
            else if(Math.abs(driverJoy.getTwist()) > .1)
            {
                jogSteerMotor(steerMtr, enc, true, invertDriveMtr, name);
                refreshSM = true;
            }
            else if(driverJoy.getRawButton(4))
            {
                Utils.pauseUntilReleased(driverJoy, 4);
                zeroEncoder(enc);
                refreshSM = true;
            }
            if(driverJoy.getRawButton(2))
            {
                Utils.pauseUntilReleased(driverJoy, 2);
                exitSM = true;
            }
        }
    }

    private static void clearLCDScreen()
    {
                 DriverStationLCD.getInstance().println(DriverStationLCD.Line.kMain6, 6, "");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 6, "");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser3, 6, "");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser4, 6, "");
                DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser5, 6, "2: Return to Main Menu");
    }

}

